import lejos.nxt.*;
import lejos.nxt.addon.CompassSensor;
import lejos.robotics.navigation.*;

public class CompassCalibrate {

	private static final float DIAMETER = 5.6F; // cm
	private static final float TRACKWIDTH = 17.8F; // cm
	private static final Motor LEFTMOTOR = Motor.B;
	private static final Motor RIGHTMOTOR = Motor.C;
	private static final SensorPort COMPASSPORT = SensorPort.S1;
	private static final boolean REVERSE = true;

	public static void main(String[] args) {
		calibrate();
	}

	public static void calibrate() {
		CompassSensor cps = new CompassSensor(COMPASSPORT);

		LCD.drawString("Press ENTER to", 0, 0);
		LCD.drawString("start calibration", 0, 1);
		while(!Button.ENTER.isPressed()){	
		}
		LCD.clear();
		LCD.drawString("Calibrating...", 0, 0);

		// Calibration
		Pilot calibratePilot = new TachoPilot(DIAMETER, TRACKWIDTH, LEFTMOTOR, RIGHTMOTOR, REVERSE);
		SimpleNavigator calibrateRobot = new SimpleNavigator(calibratePilot);
		calibrateRobot.setTurnSpeed(10);
		
		cps.startCalibration();
		calibrateRobot.rotate(720);
		cps.stopCalibration();
		
		calibrateRobot.stop();
	}
	
}
